/*
   Ultrasonic sensor Pins:
         VCC: +5V
        Trig: Trigger (INPUT) - Pin 13
        Echo: Echo (OUTPUT)   - Pin 12
         GND: GND
*/
#ifndef UC_H
#define UC_H

#include "component_l298n.h"

int trigPin = 13; // Trigger
int echoPin = 12; // Echo
long duration;
int distance_cm;

void setup_ultrasonic()
{
  // Define inputs and outputs
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);
}

void loop_ultrasonic()
{
  // The sensor is triggered by a HIGH pulse of 10 or more microseconds.
  // Give a short LOW pulse beforehand to ensure a clean HIGH pulse:
  digitalWrite(trigPin, LOW);
  delayMicroseconds(5);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);

  // Read the signal from the sensor: a HIGH pulse whose
  // duration is the time (in microseconds) from the sending
  // of the ping to the reception of its echo off of an object.
  pinMode(echoPin, INPUT);
  duration = pulseIn(echoPin, HIGH);

  // Convert the time into a distance
  distance_cm = (duration / 2) / 29.1; // Divide by 29.1 or multiply by 0.0343

  Serial.print(distance_cm);
  Serial.print("cm");
  Serial.println();

  // obstacle avoidances
  if (car_state == CAR_FORWARD)
  {
    if (distance_cm < 40)
    {
      car_action(CAR_STOP);
    }
  }
}

#endif /* UC_H */